/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/**************************************************************************
*
* NAME
*       PCtoLSF (formerly pctolsp2)
*
* FUNCTION
*
*       Compute LSF from predictor polynomial.
*
*       NOTE:  Much faster conversion can be performed
*              if the LSP quantization is incorporated.
*
* SYNOPSIS
*
*       PCtoLSF(a,freq)
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       a               fxpt_16 i       a-polynomial a(0)=1
*       freq            fxpt_16 o       lsp frequencies
*
***************************************************************************
*
* DESCRIPTION
*
*       Compute line spectrum frequencies by disection method as described in:
*
*       Line Spectrum Pair (LSP) and Speech Data Compression,
*       F.K. Soong and B-H Juang,
*       Proc. ICASSP 84, pp. 1.10.1-1.10.4
*
*       CELP's LPC predictor coefficient convention is:
*              p+1         -(i-1)
*       A(z) = SUM   a   z          where a  = +1.0
*              i=1    i                    1
*
*       Peter uses n=128, eps=1.e-06, nb=15 (this appears to be overkill!)
*
***************************************************************************
*
* CALLED BY
*
*       LPC_Analysis
*
**************************************************************************/
#define N       128
#define NB      15
#define EPS     33	/* 1.e-6 in 6.25 */
#define FALSE   0
#define TRUE    1

//#include <math.h>
#include "main.h"
#include "pctolsf.h"

void PCtoLSF(
fxpt_16 a[ORDER+1],                     /* 2.13 format */
fxpt_16 freq[ORDER+1])                  /* 0.15 format */
{
        static fxpt_16 lastfreq[ORDER]; /* 0.15 format */
        fxpt_32 p[ORDER], q[ORDER];     /* 6.25 format */
        fxpt_32 fr, tfr, fm;			/* 0.31 format */
        fxpt_32 pxl, pxm, pxr, tpxr;    /* 6.25 format */
        fxpt_16 tempfreq;				/* 0.15 format */
        fxpt_32 fl;                     /* 3.28 format */
        fxpt_32 qxl, tqxl, qxr, tqxr;   /* 6.25 format */
        fxpt_32 qxm;                    /* 6.25 format */
        fxpt_32 num, den;
        short mp, mh, nf, mb, jc, i, j, lspflag;
		fxpt_32 tmp32;

        FXPT_PUSHSTATE("PCtoLSF", 5.0, 100000000.0);
        mp = ORDER + 1;
        mh = ORDER / 2;

        /* generate p and q polynomials */

        for (i = 0; i < mh; i++) {
                p[i] = fxpt_add32(fxpt_shr32_fast(fxpt_deposit_h( a[i+1] ),4), fxpt_shr32_fast(fxpt_deposit_h( a[ORDER-i] ), 4 ));
                q[i] = fxpt_sub32(fxpt_shr32_fast(fxpt_deposit_h( a[i+1] ),4), fxpt_shr32_fast(fxpt_deposit_h( a[ORDER-i] ), 4 ));
        }

        /* compute p at f=0 */

        fl = 0;
        pxl = fxpt_deposit_h(512);     /* 1.0 in 6.25 format */
        for (j = 0; j < mh; j++) {
                /*pxl += p[j];*/
                pxl = fxpt_add32(pxl, p[j]);
        }

        /* search for zeros of p */

        nf = 0;
        for (i = 1; i <= N; pxl = tpxr, fl = tfr, i++) {
                mb = 0;
                /*fr = i * (0.5 / N);*/
                // fr = fxpt_mult16_fix(i, 128, 0);     /* fr in 0.15 fmt */
                fr = fxpt_mult64_fix(i, 128 << 16, 0);  /* fr in 0.31 fmt */
                /*pxr = cos(mp * pi * fr);*/
				pxr = fxpt_shr32_fast(fxpt_cos32(fxpt_mult64_fix(mp,fr,0)), 6);
                for (j = 0; j < mh; j++) {
                        jc = mp - (j+1)*2;
                        /*ang = jc * pi * fr;*/
                        /*pxr += cos(ang) * p[j];*/
						pxr = fxpt_add32(pxr, fxpt_mult64_fix(
							fxpt_cos32(fxpt_mult64_fix(jc, fr, 0)), p[j], 31));
                }
                tpxr = pxr;
                tfr = fr;
                if ((pxl > 0 && pxr > 0) || (pxl < 0 && pxr < 0))
                        continue;

                do {
                        mb++;
                        /*fm = fl + (fr-fl) / (pxl-pxr) * pxl;*/
                        num = fxpt_sub32(fr, fl);               /* 0.31 */
                        den = fxpt_sub32(pxl, pxr);             /* 6.25 */
                        fm = fxpt_add32(fl, fxpt_mult64_fix(num,
                            fxpt_div32(pxl, den, 31), 31));
                        /*pxm = cos(mp * pi * fm);*/
//						pxm = fxpt_shr32_fast(fxpt_cos32(fxpt_mult64_fix(mp,
//							fm, 0)), 6);
						pxm = fxpt_shr32_fast(fxpt_cos32(mp*fm), 6);

                        for (j = 0; j < mh; j++) {
                                jc = mp - (j+1) * 2;
                                /*ang = jc * pi * fm;*/
                                /*pxm += cos(ang) * p[j];*/
//								pxm = fxpt_add32(pxm, fxpt_mult64_fix(
//									fxpt_cos32(fxpt_mult64_fix(jc, fm, 0)),
//									p[j], 31));
								pxm = fxpt_add32(pxm, fxpt_mult64_round_good(
									fxpt_cos32(jc*fm),
									p[j], 31));
                        }
                        if ((pxl > 0 && pxm > 0) || (pxl < 0 && pxm < 0)) {
                                pxl = pxm;
                                fl = fm;
                        }
                        else {
                                pxr = pxm;
                                fr = fm;
                        }

                } while ((fxpt_abs32(pxm) > EPS) && (mb < 4));

                if (pxl == pxr) {
                        for (j = 0; j < ORDER; j++)
                                /*freq[j] = (j+1) * 0.04545;*/
                                freq[j] = (j+1)*1489;
                        CELP_PRINTF(("pctolsf: default lsps used, avoiding /0\n"));
                        FXPT_POPSTATE();
                        return;
                }
                /*freq[nf] = fl + (fr-fl) / (pxl-pxr) * pxl;*/
                num = fxpt_sub32(fr,fl);           /* 0.31 */
                den = fxpt_sub32(pxl,pxr);         /* 6.25 */
                freq[nf] = fxpt_add16(fxpt_extract_h(fl),
                    fxpt_mult16_round(fxpt_extract_h(num),
                    fxpt_div16(pxl, den, 15), 15));

                nf += 2;
                if (nf > ORDER-2)
					break;
        }

        /* search for the zeros of q(z) */

        freq[ORDER] = 16384;            /* 0.5 in 0.15 format */
        fl = freq[0];					/* fr, fl, fm are now 16-bit!!! */
        /*qxl = sin(mp * pi * fl);*/
        qxl = fxpt_shr32_fast(fxpt_sine32(
            (mp*fl)<<16), 6);

        for (j = 0; j < mh; j++) {
                jc = mp - (j+1) * 2;
                /*ang = jc * pi * fl;*/
                /*qxl += sin(ang) * q[j];*/
				tmp32= fxpt_sine32((jc*fl)<<16);
                qxl = fxpt_add32(qxl, fxpt_mult64_fix(
                    tmp32, q[j], 31));
        }

        for (i = 2; i < mp; qxl = tqxr, fl = tfr, i += 2) {
                mb = 0;
                fr = freq[i];
                /*qxr = sin(mp * pi * fr);*/
                qxr = fxpt_shr32_fast(fxpt_sine32(
                    (mp*fr)<<16), 6);
                for (j = 0; j < mh; j++) {
                        jc = mp - (j+1) * 2;
                        /*ang = jc * pi * fr;*/
                        /*qxr += sin(ang) * q[j];*/
						tmp32= fxpt_sine32((jc*fr)<<16);
						qxr= fxpt_add32( qxr, fxpt_mult64_round_good(
                            tmp32, q[j], 31));
                }
                tqxl = /*fxpt_abs32*/( fxpt_mult64_round_good( EPS, qxl, 25 ) );	// bigmac "fix"??? // WARNING WARNING
                tfr = fr;
                tqxr = qxr;

                do {
                        mb++;
                        /*fm = (fl+fr) * 0.5;*/
                        fm = fxpt_add32( fxpt_shr32_fast(fl,1), fxpt_shr32_fast(fr,1) );
                        //fm = fxpt_shr32_fast(fxpt_add32( fl,fr),1);
                        /*qxm = sin(mp * pi * fm);*/
                        qxm = fxpt_shr32_fast(fxpt_sine32((mp*fm)<<16), 6);

                        for (j = 0; j < mh; j++) {
                                jc = mp - (j+1) * 2;
                                /*ang = jc * pi * fm;*/
                                /*qxm += sin(ang) * q[j];*/
								tmp32= fxpt_sine32((jc*fm)<<16);
								qxm = fxpt_add32(qxm, fxpt_mult64_round_good(
									tmp32, q[j], 31));
                        }
                        if ((qxm > 0 && qxl > 0) || (qxm < 0 && qxl < 0)) {
                                qxl = qxm;
                                fl = fm;
                        }
                        else {
                                qxr = qxm;
                                fr = fm;
                        }
                } while ((fxpt_abs32(qxm) > tqxl) && (mb < NB));

                if (qxl == qxr || qxl==0 ) {
                        for (j = 0; j < ORDER; j++)
                                freq[j] = lastfreq[j];
                        CELP_PRINTF(("pctolsf: last lsps used, avoiding /0\n"));
                        FXPT_POPSTATE();
                        return;
                }
                /*freq[i-1] = fl + (fr-fl) / (qxl-qxr) * qxl;*/
                num = fxpt_sub32(fr,fl);                /* 16.15 */
                den = fxpt_sub32(qxl,qxr);				/* 6.25 */
                freq[i-1] = fxpt_add16(fxpt_extract_l(fl),
					fxpt_mult16_round(fxpt_extract_l(num),
					fxpt_div16(qxl, den, 15), 15));
        }

        /* ill-conditioned cases */

        lspflag = FALSE;
        if (freq[0] == 0 || freq[0] == 16384)   /* 16384 is 0.5 in 0.15 fmt */
                lspflag = TRUE;
        for (i = 1; i < ORDER; i++) {
                if (freq[i] == 0 || freq[i] == 16384)
                        lspflag = TRUE;

                /* reorder lsps if non-monotonic        */

                if (freq[i]  <  freq[i-1]) {
                        lspflag = TRUE;
                        CELP_PRINTF(("pctolsf: non-monotonic lsps\n"));
                        tempfreq = freq[i];
                        freq[i] = freq[i-1];
                        freq[i-1] = tempfreq;
                }
        }

        /* if non-monotonic after 1st pass, reset to last values        */

        for (i = 1; i < ORDER; i++) {
                if (freq[i]  <  freq[i-1]) {
                        CELP_PRINTF(("pctolsf: Reset to previous lsp values\n"));
                        for (j = 0; j < ORDER; j++)
                                freq[j] = lastfreq[j];
                        break;
                }
        }
        for (i = 0; i < ORDER; i++)
                lastfreq[i] = freq[i];
        FXPT_POPSTATE();
}

